// *SDA to Analog pin 4 and SCL to analog pin 5

#include <Wire.h>

unsigned long fDistanceTime = 0;
unsigned long rDistanceTime = 0;
unsigned long compassTime = 0;

boolean turning = false;
boolean DEBUG = false;
int baseHeadings[4];

// Compass variables
int HMC6352Address = 0x42;	  // I2C address of HM6352 compass module
int slaveAddress;
byte headingData[2];
int i, headingValue;
int heading = 0;
int desiredHeading = 0;
boolean mustTurnPastZero = false;

int frontDistance = 0;
int rightDistance = 0;

// Ultrasonic variables
int frontSensorPin = 0;         // Pin 0 reads input values from Maxbotics LV-EZ3 sensor
int rightSensorPin = 1;
int units  = 0;         	// Variable to store the analog value read
int inches = 0;        		// Distance to nearest object, in inches
int Distance = 0;

int RIGHT   = 0;
int FRONT   = 1;
int LEFT    = 2;
int FORWARD = 3;
int REVERSE = 4;
int STOP    = 5;

// Motor variables
int motor1Pin = 3;              // H-bridge leg 1 
int motor2Pin = 4;              // H-bridge leg 2
int motor3Pin = 5;
int motor4Pin = 6;
int speedPin1 = 9;              // H-bridge enable pin 
int speedPin2 = 10;

void setup()
{
  Serial.begin(9600);
  analogReference(DEFAULT);              // Set the analog reference to 5V = 1023

  // Shift the device's documented slave address (0x42) 1 bit right
  // This compensates for how the TWI library only wants the
  // 7 most significant bits (with the high bit padded with 0)
  slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
  Wire.begin();
	
  pinMode(motor1Pin, OUTPUT); 
  pinMode(motor2Pin, OUTPUT);
  pinMode(motor3Pin, OUTPUT);
  pinMode(motor4Pin, OUTPUT);
  pinMode(speedPin1, OUTPUT);
  pinMode(speedPin2, OUTPUT);

  digitalWrite(speedPin1, HIGH); 
  digitalWrite(speedPin2, HIGH);
  
  delay(5000);    // wait 5 seconds before we begin
  
  
  drive(FORWARD, 255);
  turn(FORWARD);
}

void loop()  {
  getDistances();
  
  if(frontDistance <= 9) {
    drive(REVERSE, 255);
    turn(RIGHT);
    delay(300);
  }
  else if((frontDistance <= 40) && (frontDistance > 9)) {
    turn(LEFT);
    drive(FORWARD, 100);
  }
  else {
    turn(FORWARD);
    drive(FORWARD, 255);
  }
  
 // Main program code goes here.
 /*Distance = distance(FRONT);
 if(Distance <= 18) {
   // backup
   drive(REVERSE);
   turn(RIGHT);
   delay(1000);
   drive(FORWARD);
   turn(LEFT);
 }
 else if((Distance <= 48) && (Distance > 18)) {
   drive(REVERSE);
   turn(LEFT);
   drive(STOP);
   delay(300);
   drive(FORWARD);
   delay(300);
   turn(FRONT);
 }
 else {
   //drive(FORWARD);
   while(distance(RIGHT) >= 36) {
     turn(RIGHT);
     delay(400);
     turn(FRONT);
   }
   turn(FRONT);
   
 }
 //Distance = distance(RIGHT);
 //if((Distance >= 24) && (Distance <=48)) {
  //turn(RIGHT);
  //}*/
}

void getDistances() {
  int currentTime = millis();
  if(currentTime >= fDistanceTime) {
    frontDistance = distance(FRONT);
    rightDistance = distance(RIGHT);
    fDistanceTime = currentTime + 75;  // Minimum required delay with .1uF cap and 100k resistor is 50ms
  }
}


int distance(int direction) {
  units = analogRead(direction);        // Read the input pin
  inches = ((units * 0.0049)/0.0098);   // (units * 0.0049) converts units to voltage, per arduino documentation
                                      	  // 0.0098 is the voltage/inch ratio provided by LV-EZ3 datasheet
  if(DEBUG) {
    Serial.print("Distance to the ");
    if(direction == FRONT) {
      Serial.print("front: ");
    }
    else if(direction == RIGHT) {
      Serial.print("right: ");
    }
    Serial.print(inches);
    Serial.println(" inches.");
  }
  //delay(100);      moved delay to another function           // Minimum required delay with .1uF cap and 100k resistor is 50ms
  return(inches);
}

int direction() {
  // Send a "A" command to the HMC6352. This requests the current heading data
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(5);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe. Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2) { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  if(DEBUG) {
    Serial.print("Current heading: ");
    Serial.print(int (headingValue / 10));     // The whole number part of the heading
    //Serial.print(".");
    //Serial.print(int (headingValue % 10));     // The fractional part of the heading
    Serial.println(" degrees.");
  }
  //delay(10);
  return(int(headingValue / 100) * 10);
}

void turn(int direction) {
  if(direction == RIGHT) {
    digitalWrite(motor3Pin, LOW);   // set leg 1 of the H-bridge low
    digitalWrite(motor4Pin, HIGH);  // set leg 2 of the H-bridge high
    if(DEBUG) {
      Serial.println("Turning right.");
    }
  }
  else if(direction == LEFT) {
    digitalWrite(motor3Pin, HIGH);  // set leg 1 of the H-bridge high
    digitalWrite(motor4Pin, LOW);   // set leg 2 of the H-bridge low
    if(DEBUG) {
      Serial.println("Turning left.");
    }
  }
  else {
    digitalWrite(motor3Pin, LOW);  // set leg 1 of the H-bridge high
    digitalWrite(motor4Pin, LOW);   // set leg 2 of the H-bridge low
    if(DEBUG) {
      Serial.println("Wheels straight.");
    }
  }
}

void drive(int direction, int speed) {
  analogWrite(speedPin1, speed);
  
  if(direction == REVERSE) {
    digitalWrite(motor1Pin, LOW);   // set leg 1 of the H-bridge low
    digitalWrite(motor2Pin, HIGH);  // set leg 2 of the H-bridge high
    if(DEBUG) {
      Serial.println("Driving in Reverse.");
    }
  }
  else if(direction == FORWARD) {
    digitalWrite(motor1Pin, HIGH);  // set leg 1 of the H-bridge high
    digitalWrite(motor2Pin, LOW);   // set leg 2 of the H-bridge low
    if(DEBUG) {
      Serial.println("Driving Forward.");
    }
  }
  else {
    digitalWrite(motor1Pin, LOW);  // set leg 1 of the H-bridge high
    digitalWrite(motor2Pin, LOW);   // set leg 2 of the H-bridge low
    if(DEBUG) {
      Serial.println("Stopping.");
    }
  }
}

